﻿using System.IO;
using System.Threading;
using GHIElectronics.NETMF.FEZ;
using GHIElectronics.NETMF.Hardware;
using GHIElectronics.NETMF.IO;
using Microsoft.SPOT.IO;
using System.Text;
using NetMf.CommonExtensions;
using Microsoft.SPOT.Hardware;
using AstroX;
using System.IO.Ports;
using Microsoft.SPOT;
using System;
using System.Diagnostics;

public class Program
{
    
    static FileStream fs;

    public static string rootDirectory = "";
    static SerialPort UART;

    public static void Main()
    {
        Debug.EnableGCMessages(false);

        //OutputPort port = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.Di1, true);
        //while (1 == 1)
        //{
        //    port.Write(!port.Read());
        //    Thread.Sleep(500);
        //}
        
        UART = new SerialPort("COM2", 19200);
        
        UART.Open();


        Print("Starting up");
        //OutputPort tp = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.Di0,true);
#if DEBUG

        Print("!!!!!!!!!!!!!!!!!!! DEBUG CODE!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
#endif
        try
        {
            PersistentStorage sdPS = new PersistentStorage("SD");

            sdPS.MountFileSystem();
            rootDirectory = VolumeInfo.GetVolumes()[0].RootDirectory;

            fs = new FileStream(Path.Combine(rootDirectory, "log.txt"), FileMode.Append);
            Print("SD initialized");
        }
        catch(Exception e)
        {
            Program.Print(e.Message);
        }

        
        FlightControl.Init(rootDirectory);        
        AccelerometerController s = new AccelerometerController();
        TemperatureController t = new TemperatureController();
        Boom b = new Boom();
        PowerControl p = new PowerControl();
        CameraControl c = new CameraControl();
        
#if DEBUG
        DiagnosticControl d = new DiagnosticControl(UART, b, p);

        Thread thread = new Thread(() =>
        {

            FlightControl.CurrentPhase = FlightPhases.StandingBy;

            Thread.Sleep(1 * 1000);
            FlightControl.CurrentPhase = FlightPhases.Ascent;

            Thread.Sleep(10 * 1000);
            FlightControl.CurrentPhase = FlightPhases.Extend;
            Thread.Sleep(60 * 1000);
            FlightControl.CurrentPhase = FlightPhases.Retract;
            Thread.Sleep(60 * 1000);
            FlightControl.CurrentPhase = FlightPhases.Shutdown;
        });
        thread.Start();
#endif
        FlightControl.CurrentPhase = FlightPhases.StandingBy;
        Thread.Sleep(Timeout.Infinite);

    }

    public static void Print(string str)
    {
        str = DateTime.Now.Ticks + ":" + str + "\r\n";
        
        
#if DEBUG
            Debug.Print(str);
#endif
        
        var buffer = Encoding.UTF8.GetBytes(str);
        UART.Write(buffer, 0, buffer.Length);
        UART.Flush();
        if (fs == null) { return; }
        fs.Write(buffer, 0, buffer.Length);
        fs.Flush();
    }
    
 
}

